//
// Created by breeze on 23/3/31.
//
#include <cmath>
#include "manager.h"
#include "func.h"

using namespace std;

bool sort_rule(const pair<TPlatform*,TPlatform*> a, const pair<TPlatform*,TPlatform*> b)
{
    double disa = sqrt((a.first->x-a.second->x)*(a.first->x-a.second->x)+((a.first->y-a.second->y)*(a.first->y-a.second->y)));
    double disb = sqrt((b.first->x-b.second->x)*(b.first->x-b.second->x)+((b.first->y-b.second->y)*(b.first->y-b.second->y)));
    return disa<disb;
}

int TManager::Init(vector<string>& buff)
{
    // 系统初始化函数
    platforms.reserve(100);
    robots.reserve(100);
    int cnt = 0;

    int cnt_1 = 0; //竖线

    int cnt_7 = 0;

    int cnt_8 = 0;
    for(int i=0; i<100; i++){  //读取平台和机器人坐标
        for(int j=0; j<100; j++){
            if(buff[i][j] == '.'){
                continue;
            }else if(buff[i][j]=='A'){
                double m_x=(double)(100-i-1)/(double)2+0.25;  //坐标转换公式
                double m_y=(double)j/(double)2 + 0.25;
                //cerr<<m_x<<"  "<<m_y<<'\n';
                auto t = new TRobot(robots.size(),m_x,m_y);
                robots.emplace_back(t);
            }else if(buff[i][j]<='9'&&buff[i][j]>='1'){
                double m_x=(double)(100-i-1)/(double)2+0.25;  //坐标转换公式
                double m_y=(double)j/(double)2 + 0.25;
                //cerr<<m_x<<"  "<<m_y<<'\n';
                int number = buff[i][j]-'0';
                if(number>=1&&number<=3){
                    auto t = new TPlatform(number, m_x, m_y);
                    platforms.emplace_back(t);
                    fast_plat.emplace(make_pair(platforms[platforms.size()-1],platforms.size()-1));
                    if(number == 1){
                        cnt_1 ++;
                    }
                    //cerr<<" %^&  "<<platforms.size()<<" "<<fast_plat[&platforms[platforms.size()-1]]]<<" "<<&platforms[platforms.size()-1]<<'\n';
                }else if(number>=4){
                    if(number==4){auto t = new TPlatform(number,m_x,m_y);platforms.emplace_back(t); fast_plat.emplace(make_pair(platforms[platforms.size()-1],platforms.size()-1));}
                    if(number==5){auto t = new TPlatform(number,m_x,m_y);platforms.emplace_back(t); fast_plat.emplace(make_pair(platforms[platforms.size()-1],platforms.size()-1));}
                    if(number==6){auto t = new TPlatform(number,m_x,m_y);platforms.emplace_back(t); fast_plat.emplace(make_pair(platforms[platforms.size()-1],platforms.size()-1));}
                    if(number==7){auto t = new TPlatform(number,m_x,m_y);platforms.emplace_back(t); fast_plat.emplace(make_pair(platforms[platforms.size()-1],platforms.size()-1));cnt_7++;}
                    if(number==8){auto t = new TPlatform(number,m_x,m_y);platforms.emplace_back(t); fast_plat.emplace(make_pair(platforms[platforms.size()-1],platforms.size()-1));cnt_8++;}
                    if(number==9){auto t = new TPlatform(number,m_x,m_y);platforms.emplace_back(t); fast_plat.emplace(make_pair(platforms[platforms.size()-1],platforms.size()-1)); cnt++;}
                    //cerr<<" %^&  "<<platforms.size()<<" "<<fast_plat[platforms[platforms.size()-1]]<<" "<<platforms[platforms.size()-1]<<'\n';
                    //cerr<<"$#################################    "<<platforms[platforms.size()-1].free_numbers.size()<<'\n';
                }
            }

        }
    }

    total_used = platforms.size();
    //分析平台分布
    // if(cnt_1 == 3 && platforms.size() == 18){
    //     for(auto p: platforms){
    //         if((p->x>35 && p->number<=3) || (p->x<20 && p->number==2 && p->y>25) || (p->x<20 && p->number==1 && p->y<25)){
    //             p->canused = 0;
    //             total_used --;
    //             avoid_on = 1;
    //         }
    //     }
    // }

    if(cnt == 4){//说明
        avoid_on = true;
        map_num= 1;
        for(auto p: platforms){
            // if(p->number == 9){
            //     p->canused = 0;
            // }
            if(p->number == 4 && (p->y>21||p->x>15)){
                p->can_use = 0;
            }
            if(p->number == 5 && (p->x<23 || p->y<26)){
                p->can_use = 0;
            }
            if(p->number == 6 && (p->y<29 || p->x>15)){
                p->can_use = 0;
            }
            if(p->number == 7 && (p->y!=23.25 || p->x!=17.25) && (p->y!=26.25 || p->x!=20.25)){//
                p->can_use = 0;
            }
        }
    }

    if(cnt_8 == 2){
        avoid_on = true;
        map_num= 2;
        for(auto p: platforms){
            if(p->y<24){
                p->can_use = 0;
            }
            if(p->y>45 && p->number!=6){
                p->can_use = 0;
            }
            if(p->x<25 && p->number == 5){
                p->can_use = 0;
            }
            if(p->y>45 && p->number==6 && p->x<25){
                p->can_use = 0;
            }

        }
    }

    if(cnt_8 == 0){
        map_num= 3;
        for(auto p: platforms){
            if(p->y<17){
                p->can_use = 0;
            }
            if(p->x<15 && p->number>3){
                p->can_use = 0;
            }
            if(p->y>41 && p->number==1){
                p->can_use = 0;
            }
            if(p->number == 2 && p->y == 27.25){
                p->can_use = 0;
            }
            // if(p->y>47){
            //     p->canused = 0;
            // }
            if(p->x>40 && p->number==5){
                p->can_use = 0;
            }
        }
    }

    if(cnt_7 == 1){
        map_num= 4;
        for(auto p: platforms){
            if((p->y>22 && p->y<27)&&p->number==3){
                p->can_use = 0;
            }
        }
    }

    for(int i=0; i<platforms.size(); i++){
        if(platforms[i]->can_use == 0)continue;
        if(platforms[i]->number<=3){
            plat_123.emplace_back(platforms[i]);
            plat_1234567.emplace_back(platforms[i]);
        }else if(platforms[i]->number>=4&&platforms[i]->number<=6){
            plat_456.emplace_back(platforms[i]);
            plat_1234567.emplace_back(platforms[i]);
        }else if(platforms[i]->number == 7){
            plat_7.emplace_back(platforms[i]);
            plat_1234567.emplace_back(platforms[i]);
        }else if(platforms[i]->number == 8){
            plat_8.emplace_back(platforms[i]);
        }else{
            plat_9.emplace_back(platforms[i]);
        }
    }
    //先不做排序，distance(cost->x,cost->y,product->x,product->y);


    FillSellAndBuyMap(plat_123,plat_456);
    FillSellAndBuyMap(plat_123,plat_9);
    FillSellAndBuyMap(plat_456,plat_7);
    FillSellAndBuyMap(plat_456,plat_9);
    FillSellAndBuyMap(plat_7,plat_8);
    FillSellAndBuyMap(plat_7,plat_9);

    for(auto &it: platforms){   //对应的从小到大排序
        if(it->can_use == 0)continue;
        for(auto& map:it->sell_map)
            sort(map.second.begin(),map.second.end(),sort_rule);
        for(auto& map:it->buy_map)
            sort(map.second.begin(),map.second.end(),sort_rule);
        // cerr<<"curr number: "<<it->number<<'\n';
        // cerr<<"**************************\n";
        // for(auto& map:it->sell_map){
        //     cerr<<" keys : "<<map.first<<'\n';
        //     for(auto& t:it->sell_map[map.first]){
        //         cerr<<" dis : "<<distance(it->x,it->y,t.second->x,t.second->y)<<'\n';
        //     }
        // }
        // cerr<<"**************************\n";
    }
    // while(1);

    return 0;
}

int TManager::Read(vector<string>& buff)
{    //帧读取函数
    int l_robot=robots.size();
    int l_platfm=platforms.size();

    cerr<<buff.size()<<" "<<l_platfm+l_robot+3<<'\n';


    if(buff.size()!=l_platfm+l_robot+3){
        cerr<<"main read error\n";
        return -1;
    }

    string b;
    for(int i=0; i<buff[0].size(); i++){
        if(buff[0][i]==' '){
            break;
        }else{
            b += buff[0][i];  //取出帧ID
            //cerr<<messages<<" "<<'\n';
        }
    }

    frameID = stoi(b);
    printf("%d\n", stoi(b));
    //cerr<<stoi(b)<<'\n';
    //cerr<<messages;

    //plat_full_345.reserve(50);  //平台生产格满了，材料格也满了/马上满了,用于买卖函数
    plat_fulland1free.reserve(50);
    plat_fulland2free.reserve(50);
    plat_freeand1.reserve(50);
    plat_freeand2.reserve(50);
    plat_busyand1.reserve(50);
    plat_busyand2.reserve(50);

    int i=2;
    for(int j=0; j<platforms.size(); j++){
        platforms[j]->Read(buff[i]);
        if(platforms[j]->can_use == 0){
            i++;
            continue;
        }

        if(platforms[j]->number <=3 || platforms[j]->number >=7){
            i++;
            continue;
        }
        // cerr<<fast_plat[platforms[j]]<<" " <<platforms[j]->free_numbers.size()<<" "<<platforms[j]->have_numbers.size()<<" "<<platforms[j]->sell_food.size()<<" "
        //         <<platforms[j]->pre_sel_food.size()<<" "<<platforms[j]->working_numbers.size()<<'\n';

        for(auto it: platforms[j]->working_numbers){
            cerr<<it.first->number<<" "<<it.second.first<<" "<<it.second.second<<'\n';
            // if(get<>(it.first->work_queue.front())){

            // }
        }

        cerr<<"###############\n";

        // if(platforms[j]->busy_time == -1 && platforms[j]->free_numbers.size() == 1){
        //     plat_freeand1.emplace_back(platforms[j]);
        // }else if(platforms[j]->busy_time == -1 && platforms[j]->free_numbers.size() == 2){
        //     plat_freeand2.emplace_back(platforms[j]);
        // }

        if(platforms[j]->sell_food.size() == 1){//满了的345平台
            plat_full_456.emplace_back(platforms[j]);
            if(platforms[j]->number == 4){
                plat_full_4.emplace_back(platforms[j]);
            }else if(platforms[j]->number == 5){
                plat_full_5.emplace_back(platforms[j]);
            }else  if(platforms[j]->number == 6){
                plat_full_6.emplace_back(platforms[j]);
            }
        }
        cerr<<"busy_time                 "<<platforms[j]->busy_time<<'\n';
        i++;
    }

    for (int j=0; j<robots.size(); j++){
        robots[j]->Read(buff[i]);

        if(robots[j]->is_busy == 0){ //无目标机器人
            robot_empty_rob.emplace_back(robots[j]);
        }else{ //有目标机器人
            if(robots[j]->platform_idx == fast_plat[get<2>(robots[j]->work_queue.front())]){//在目标平台里
                robot_pay_buy.emplace_back(robots[j]);
                //while(1);
            }else{
                busy_robot_target_platform[robots[j]] = fast_plat[get<2>(robots[j]->work_queue.front())];
            }
        }
        i++;
    }

    cerr<<busy_robot_target_platform.size()<<" "<<robot_pay_buy.size()<<" "<<robot_empty_rob.size()<<" "<<'\n';
    return 0;
}

void TManager::Write()
{
    // 帧回复函数
    //cerr<<("OK\n")<<'\n';
    for (int i = 0; i < destroy.size(); i++)
        printf("destroy %d\n", destroy[i]->number);

    for (auto r: robots)
    {
        printf("forward %d %f\n", r->number, r->v_set);
        printf("rotate %d %f\n", r->number, r->rotate_set);
    }

    printf("OK\n");
    //fflush(stdout);
}

void TManager::Clear()
{              //清空对象中的无用容器
    //cerr<<"clear message:\n"<<messages;
    robot_pay_buy.clear();
    robot_empty_rob.clear();

    busy_robot_target_platform.clear();

    // plat_fullandnofree.clear();  //平台生产格满了，材料格也满了/马上满了
    // plat_fulland1free.clear();
    plat_fulland2free.clear();
    plat_freeand1.clear();
    plat_freeand2.clear();
    plat_busyand1.clear();
    plat_busyand2.clear();
    plat_full_456.clear();

    plat_full_4.clear();
    plat_full_5.clear();
    plat_full_6.clear();

    destroy.clear();

}

void TManager::RunOnce(vector<string> &frame)
{
    Read(frame);

    if (map_num == 1)
        DoDeal1();
    else if(map_num == 2)
        DoDeal2();
    else if(map_num == 3)
        DoDeal3();
    else if(map_num == 4)
        DoDeal4();

    DealPayBuy();
    ControlRobot();
    Write();
    Clear();
}

void TManager::ControlRobot()
{
    // 操作机器人，包括已经到了工作台附近的
    // if(busy_robot_target_platform.size()+robot_pay_buy.size()+destroy.size() != robots.size()){
    //     for(auto it: busy_robot_target_platform){
    //         cerr<<" "<<it.first->number<<'\n';
    //     }

    //     cerr<<"err control    "<<busy_robot_target_platform.size()<<" "<<robots.size()<<" "<<robot_pay_buy.size()<<" "<<destroy.size()<<'\n';
    //     while(1);
    // }
    cerr<<"busy_robot_target_platform_size  "<<busy_robot_target_platform.size()<<'\n';

    for(auto it:busy_robot_target_platform){
        if(it.first->work_queue.size() == 0)continue;
        TPlatform* buffpl = get<2>(it.first->work_queue.front());
        TRobot*   buffro = it.first;
        if(buffpl == nullptr || buffro == nullptr){
            continue;
        }

        double rotate;
        int v;

        double rx = buffro->x;
        double ry = buffro->y;
        double rdis = buffro->radius;

        double plx = buffpl->x;
        double ply = buffpl->y;

        double rvx = buffro->line_v[0];
        double rvy = buffro->line_v[1];

        double tardirect = atan2(ply - ry, plx - rx); //机器人-工作台角度
        double direction = buffro->direction;        //机器人朝向
        double vdirecton = atan2(rvy, rvx) ;

        //double direction_dplusv = direction - vdirecton; //朝向速度的夹角
        double direction_dplust = AngleDiff(direction,tardirect) / M_PI * 180; //朝向和目标的夹角
        //double direction_vplust = vdirecton - tardirect; //速度与目标的夹角


        double total_v = sqrt((rvy*rvy)+(rvx*rvx));
        double dist = Distance(rx, ry, plx, ply); // 计算当前距离
        // double targetAngle = atan2(ply - ry, plx - rx);// 计算需要朝向的角度
        // double diff = angleDiff(direction, tardirect); // 计算当前朝向与目标朝向的角度差

        //构建扫描面积容器
        vector<vector<pair<double,double>>> scan_line(robots.size(),vector<pair<double,double>> (2,make_pair(0,0)));  //存放接下来的扫描面积
        vector<pair<double,double>> memo_v_r;  //存放接下来的《速度,旋转速度》

        cerr<<"################jiaodu "<<" "<<direction_dplust<<" "<<'\n';

        cerr<<"==========================="<<" "<<direction_dplust<<" "<<total_v<<"   "<<dist<<'\n';
        if(fabs(direction_dplust)<3){
            rotate = 0;
            v = 6;
            //if(buffpl->number<=3 && map_num == 1)v = 3*dist+1;

            if(map_num == 2){
                if(buffpl->number < 7 && buffpl->number >= 5 && dist < 2){
                    v = dist*2+1;
                }else if(buffpl->number <= 4 && dist < 2 && map_num == 2){
                    v = 5;
                }else{
                    v = 6;
                }
            }

            // if(map_num == 3){
            // if(dist < 2 && map_num == 3 && buffpl->number != 9){
            //     v = 4;
            // }else{
            //     v = 6;
            // }}

            if(map_num == 1 && buffpl->number<=3){
                v = dist*2+1;
            }
        }else if(fabs(direction_dplust)<8){//不调，全速前进
            if(direction_dplust>0){
                rotate = +M_PI/6; //*((*3+1)
                if(map_num == 2)rotate = +M_PI * 0.75;
                if(map_num == 3)rotate = +M_PI * 0.75;
            }else{
                rotate = -M_PI/6;
                if(map_num == 2)rotate = -M_PI * 0.75;
                if(map_num == 3)rotate = -M_PI * 0.75;
            }
            // rotate = rotate * ((fabs(direction_dplust)-1)/5);

            v = 6;             //速度高了
            if(dist < 2 && map_num == 2){
                v = 3;
            }else{
                v = 6;
            }
        }else if(fabs(direction_dplust)<45){ //微调，加高速
            if(direction_dplust>0){
                rotate = +M_PI/3; //*((*3+1)
                if(map_num == 1 || map_num == 2 || map_num == 3)rotate = +M_PI;
            }else{
                rotate = -M_PI/3;
                if(map_num == 1 || map_num == 2 || map_num == 3)rotate = -M_PI;
            }

            v = 6;             //速度高了

            //while(1);
        }else if(fabs(direction_dplust)<90){//全力调，减速为0
            if(direction_dplust>0){
                rotate = +M_PI;
            }else{
                rotate = -M_PI;
            }

            v = 4;             //速度高了
            if(total_v <= 6 && total_v>=5)v = total_v;  //保持合适速度
            if(total_v<3)v = 6;           //速度低了

        }else if(fabs(direction_dplust)<180){//全力调，减速为0
            if(direction_dplust>0){
                rotate = +M_PI;
            }else{
                rotate = -M_PI;
            }

            v = 0;             //速度高了
            if(total_v <= 1 && total_v>0.5)v = total_v;  //保持合适速度
            if(total_v<0.5)v = 0.5;           //速度低了
            if(map_num != 3)v = -1;
        }
        // else{
        //     cerr<<"rotate and v error\n";
        //     while(1);
        // }
        //cout<<rotate <<" "<<v<<'\n';
        //while(1);
        int K = 0;
        if(platforms.size() == 18){
            K = 250;
        }

        if(frameID > K && avoid_on == 1 && frameID<8800){
            Scanner(scan_line,memo_v_r);
            Avoid(v,rotate,it,scan_line,memo_v_r);
            // control_avoid(it.first,rotate,v);
        }

        if(fabs(direction_dplust) <105 && fabs(direction_dplust)>=90 && dist<1.1 && dist>0.8){//默认为旋转状态
            if(direction_dplust>0){
                rotate = +M_PI;
            }else{
                rotate = -M_PI;
            }

            if(map_num == 2)v = -1;
            else v = -2;
        }

        if(total_v < 5 && dist<2 && fabs(direction_dplust) >45){//默认为旋转状态改变过程
            if(direction_dplust>0){
                rotate = +M_PI;
            }else{
                rotate = -M_PI;
            }

            if(map_num == 2)v = -1;
            else v = -2;
        }


        cerr<<"^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^      "<<rotate<<" "<<v<<'\n';

        it.first->v_set = v;
        it.first->rotate_set = rotate;
//        printf("forward %d %d\n", it.first->number,v);
//        printf("rotate %d %f\n", it.first->number,(float)rotate);
    }
}

void TManager::DoDeal1()
{

    // if(plat_7.size() == 2){//7离得近，效率高
    for(auto it: plat_7)
    {
        //最近的空机器人，最近结束工作的机器人安排任务
        SellMonitorPlat7(it);
    }

    // for(auto it: robots){//监控是否有未取到货的机器人
    //     monitor_robot(it);
    // }


    //检查共同目标的机器人是否存在买卖关系,针对要走很远的图
    // for(auto ro:robots){
    //     if(frameID > 8700)continue;
    //     if(ro->work_queue.size() != 2)continue;
    //     if(distance(ro->x,ro->y,get<2>(ro->work_queue.front())->x,get<2>(ro->work_queue.front())->y)>5 && get<2>(ro->work_queue.front())->working_numbers.size()>1 && get<0>(ro->work_queue.front())==1 && get<1>(ro->work_queue.front())>3){ //该机器人为买机器人，存在另一个机器人去卖东西
    //         Transfer_Task(ro,get<2>(ro->work_queue.front())->working_numbers);//内部已经弹出了一个wq
    //     }
    // }

    int get_full_robot = 0;//得到full机器人的分配坐标
    cerr<<" &&&&&&   "<<plat_full_456.size()<<" "<<plat_456.size()<<" "<<get_full_robot<<'\n';
    if(frameID > 2500) get_full_robot = max(get_full_robot,1);
    if(frameID > 6500) get_full_robot = 4;
    if(map_num == 1)get_full_robot = 3;

    for(int i=0; i<get_full_robot; i++){//搬运full的机器人
        if(robots[i]->work_queue.size() == 0){
            FullFirst(robots[i]);  //只处理456
        }
        if(robots[i]->work_queue.size() == 0){
            DistanceFirst(robots[i]);  //只处理123 + 加急卖7
        }
    }
    cerr<<" &&&&&&   "<<get_full_robot<<'\n';
    for(int i=get_full_robot; i<robots.size(); i++){
        if(robots[i]->work_queue.size() == 0){
            DistanceFirst(robots[i]);
        }
        if(robots[i]->work_queue.size() == 0){
            FullFirst(robots[i]);  //只处理456
        }
    }
}

void TManager::DoDeal2()
{
    // if(plat_7.size() == 2){//7离得近，效率高
    // for(auto it: plat_7){//最近的空机器人，最近结束工作的机器人安排任务
    //     sell_monitor_plat7(it);
    // }

    // for(auto it: robots){//监控是否有未取到货的机器人
    //     monitor_robot(it);
    // }

    // for(auto it: robots){//主动检查是否可分配456
    //     auto work_q = it->work_queue;
    //     if(it->work_queue.size() == 0)continue;

    //     //查看是否路过，并且最后的目标是卖给他,提前分配任务
    //     if(get<0>(work_q.back()) == 0 && get<2>(work_q.back())->number>=4 && get<2>(work_q.back())->sell_food.size() == 1){
    //         if(get<2>(work_q.back())->number<7)pla_sell_task_notake_456_79(get<2>(work_q.back()),it);
    //         else pla_sell_task_notake_7_89(get<2>(work_q.back()),it);
    //     }
    // }
    // }else {
    cerr<<"444444444444444\n";
    for(auto it: robots){//主动检查是否可分配456
        auto work_q = it->work_queue;
        if(it->work_queue.size() == 0)continue;

        //查看是否路过，并且最后的目标是卖给他,提前分配任务
        if(get<0>(work_q.back()) == 0 && get<2>(work_q.back())->number>=4 && get<2>(work_q.back())->sell_food.size() == 1){
            if(get<2>(work_q.back())->number<7)
                PlaSellTaskNoTake_456_79(get<2>(work_q.back()),it);//9号平台只能有2个同时送
            else
                PlaSellTaskNoTake_7_89(get<2>(work_q.back()),it);
        }
        cerr<<"55555555555555\n";
    }
    cerr<<"666666666666\n";
    for(auto it: plat_7){//最近的空机器人，最近结束工作的机器人安排任务
        SellMonitorPlat7(it);
    }

    for(auto it: robots){//监控是否有未取到货的机器人
        MonitorRobot(it);
    }
    // }

    //检查共同目标的机器人是否存在买卖关系,针对要走很远的图
    // for(auto ro:robots){
    //     if(frameID > 8000)continue;
    //     if(ro->work_queue.size() != 2)continue;
    //     if(distance(ro->x,ro->y,get<2>(ro->work_queue.front())->x,get<2>(ro->work_queue.front())->y)>5 && get<2>(ro->work_queue.front())->working_numbers.size()>1 && get<0>(ro->work_queue.front())==1 && get<1>(ro->work_queue.front())>3){ //该机器人为买机器人，存在另一个机器人去卖东西
    //         Transfer_Task(ro,get<2>(ro->work_queue.front())->working_numbers);//内部已经弹出了一个wq
    //     }
    // }

    int get_full_robot = 0;//得到full机器人的分配坐标
    cerr<<" &&&&&&   "<<plat_full_456.size()<<" "<<plat_456.size()<<" "<<get_full_robot<<'\n';
    //if(frameID > 4500) get_full_robot = max(get_full_robot,2);
    if(frameID > 6500) get_full_robot = 4;
    if(map_num == 1)get_full_robot = 2;

    for(int i=0; i<get_full_robot; i++){//搬运full的机器人
        if(robots[i]->work_queue.size() == 0){
            FullFirst(robots[i]);  //只处理456
        }
        if(robots[i]->work_queue.size() == 0){
            DistanceFirst(robots[i]);  //只处理123 + 加急卖7
        }
    }
    cerr<<" &&&&&&   "<<get_full_robot<<'\n';
    for(int i=get_full_robot; i<robots.size(); i++){
        if(robots[i]->work_queue.size() == 0){
            DistanceFirst(robots[i]);
        }
        if(robots[i]->work_queue.size() == 0){
            FullFirst(robots[i]);  //只处理456
        }
    }
}

void TManager::DoDeal3()
{

    cerr<<"444444444444444\n";
    for(auto it: robots){//主动检查是否可分配456
        auto work_q = it->work_queue;
        if(it->work_queue.size() == 0)continue;

        //查看是否路过，并且最后的目标是卖给他,提前分配任务
        if(get<0>(work_q.back()) == 0 && get<2>(work_q.back())->number>=4 && get<2>(work_q.back())->sell_food.size() == 1){
            if(get<2>(work_q.back())->number<7)
                PlaSellTaskNoTake_456_79(get<2>(work_q.back()),it);//9号平台只能有2个同时送
            else
                PlaSellTaskNoTake_7_89(get<2>(work_q.back()),it);
        }
        cerr<<"55555555555555\n";
    }
    cerr<<"666666666666\n";

    for(auto it: robots){//监控是否有未取到货的机器人
        MonitorRobot(it);
    }
    // }

    //检查共同目标的机器人是否存在买卖关系,针对要走很远的图
    // for(auto ro:robots){
    //     if(frameID > 8700)continue;
    //     if(ro->work_queue.size() != 2)continue;
    //     if(distance(ro->x,ro->y,get<2>(ro->work_queue.front())->x,get<2>(ro->work_queue.front())->y)>5 && get<2>(ro->work_queue.front())->working_numbers.size()>1 && get<0>(ro->work_queue.front())==1 && get<1>(ro->work_queue.front())>3){ //该机器人为买机器人，存在另一个机器人去卖东西
    //         Transfer_Task(ro,get<2>(ro->work_queue.front())->working_numbers);//内部已经弹出了一个wq
    //     }
    // }

    int get_full_robot = 0;//得到full机器人的分配坐标
    cerr<<" &&&&&&   "<<plat_full_456.size()<<" "<<plat_456.size()<<" "<<get_full_robot<<'\n';
    if(frameID > 1500) get_full_robot = max(get_full_robot,1);
    if(frameID > 3500) get_full_robot = 2;
    if(frameID > 6500) get_full_robot = 3;

    for(int i=0; i<get_full_robot; i++){//搬运full的机器人
        if(robots[i]->work_queue.size() == 0){
            FullFirst(robots[i]);  //只处理456
        }
        if(robots[i]->work_queue.size() == 0){
            DistanceFirst(robots[i]);  //只处理123 + 加急卖7
        }
    }
    cerr<<" &&&&&&   "<<get_full_robot<<'\n';
    for(int i=get_full_robot; i<robots.size(); i++){
        if(robots[i]->work_queue.size() == 0){
            DistanceFirst(robots[i]);
        }
        if(robots[i]->work_queue.size() == 0){
            FullFirst(robots[i]);  //只处理456
        }
    }
}

void TManager::DoDeal4()
{

    // if(plat_7.size() == 2){//7离得近，效率高
    // for(auto it: plat_7){//最近的空机器人，最近结束工作的机器人安排任务
    //     sell_monitor_plat7(it);
    // }

    // for(auto it: robots){//监控是否有未取到货的机器人
    //     monitor_robot(it);
    // }

    // for(auto it: robots){//主动检查是否可分配456
    //     auto work_q = it->work_queue;
    //     if(it->work_queue.size() == 0)continue;

    //     //查看是否路过，并且最后的目标是卖给他,提前分配任务
    //     if(get<0>(work_q.back()) == 0 && get<2>(work_q.back())->number>=4 && get<2>(work_q.back())->sell_food.size() == 1){
    //         if(get<2>(work_q.back())->number<7)pla_sell_task_notake_456_79(get<2>(work_q.back()),it);
    //         else pla_sell_task_notake_7_89(get<2>(work_q.back()),it);
    //     }
    // }
    // }else {
    cerr<<"444444444444444\n";
    for(auto it: robots){//主动检查是否可分配456
        auto work_q = it->work_queue;
        if(it->work_queue.size() == 0)continue;

        //查看是否路过，并且最后的目标是卖给他,提前分配任务
        if(get<0>(work_q.back()) == 0 && get<2>(work_q.back())->number>=4 && get<2>(work_q.back())->sell_food.size() == 1){
            if(get<2>(work_q.back())->number<7)
                PlaSellTaskNoTake_456_79(get<2>(work_q.back()),it);//9号平台只能有2个同时送
            else
                PlaSellTaskNoTake_7_89(get<2>(work_q.back()),it);
        }
        cerr<<"55555555555555\n";
    }
    cerr<<"666666666666\n";
    for(auto it: plat_7){//最近的空机器人，最近结束工作的机器人安排任务
        SellMonitorPlat7(it);
    }

    for(auto it: robots){//监控是否有未取到货的机器人
        MonitorRobot(it);
    }
    // }

    //检查共同目标的机器人是否存在买卖关系,针对要走很远的图
    // for(auto ro:robots){
    //     if(frameID > 8700)continue;
    //     if(ro->work_queue.size() != 2)continue;
    //     if(distance(ro->x,ro->y,get<2>(ro->work_queue.front())->x,get<2>(ro->work_queue.front())->y)>5 && get<2>(ro->work_queue.front())->working_numbers.size()>1 && get<0>(ro->work_queue.front())==1 && get<1>(ro->work_queue.front())>3){ //该机器人为买机器人，存在另一个机器人去卖东西
    //         Transfer_Task(ro,get<2>(ro->work_queue.front())->working_numbers);//内部已经弹出了一个wq
    //     }
    // }

    int get_full_robot = 0;//得到full机器人的分配坐标
    cerr<<" &&&&&&   "<<plat_full_456.size()<<" "<<plat_456.size()<<" "<<get_full_robot<<'\n';
    get_full_robot = max(get_full_robot,4);

    for(int i=0; i<get_full_robot; i++){//搬运full的机器人
        if(robots[i]->work_queue.size() == 0){
            FullFirst(robots[i]);  //只处理456
        }
        if(robots[i]->work_queue.size() == 0){
            DistanceFirst(robots[i]);  //只处理123 + 加急卖7
        }
    }
    cerr<<" &&&&&&   "<<get_full_robot<<'\n';
    for(int i=get_full_robot; i<robots.size(); i++){
        if(robots[i]->work_queue.size() == 0){
            DistanceFirst(robots[i]);
        }
        if(robots[i]->work_queue.size() == 0){
            FullFirst(robots[i]);  //只处理456
        }
    }
}

void TManager::TransferTask(TRobot* ro,multimap<TRobot *, std::pair<bool, int>>& wn)
{//前者是需要转让的，后者为对应pla的工作队列
    for(auto it: wn){
        if(it.first!=ro){
            TRobot* target_ro = it.first;
            target_ro->work_queue.emplace(ro->work_queue.front());
            wn.emplace(target_ro,make_pair(get<0>(ro->work_queue.front()),get<1>(ro->work_queue.front())));
            EraseWorkingNumber(wn,ro->work_queue.front(),ro);
            ro->work_queue.pop();

            // if(ro->work_queue.size()!=0 && ro->work_queue.size()!=1){
            //     cerr<<"Transfer_Task error\n";
            //     while(1);
            // }

            target_ro->work_queue.emplace(ro->work_queue.front());
            if(get<1>(ro->work_queue.front())!=7){
                get<2>(ro->work_queue.front())->working_numbers.emplace(target_ro,make_pair(get<0>(ro->work_queue.front()),get<1>(ro->work_queue.front())));
                EraseWorkingNumber(get<2>(ro->work_queue.front())->working_numbers,ro->work_queue.front(),ro);
            }
            ro->work_queue.pop();

            // if(ro->work_queue.size()!=0 && ro->work_queue.size()!=1){
            //     cerr<<"Transfer_Task error\n";
            //     while(1);
            // }
            break;
        }
    }
}

void TManager::DealPayBuy()
{
    // 处理机器人买卖才做，但没买到的
    int rob_pay_buy_sz = robot_pay_buy.size();//记录可以卖的机器人的数量
    if(rob_pay_buy_sz == 0) return;
    for(int i = 0; i < rob_pay_buy_sz;i++){
        //     while (1)
        // {
        //     /* code */
        // }
        //get<2>(robot_pay_buy[i]->work_queue.front())->busy_time = 1;  //防止同时进入,这代码不能用，因为我们没读1,2,3,8,9号
        cerr<<"-------------------------------------------------------"<<'\n';

        // if(get<0>(robot_pay_buy[i]->work_queue.front()) == 1 && get<2>(robot_pay_buy[i]->work_queue.front())->busy_time>0 && get<2>(robot_pay_buy[i]->work_queue.front())->number == 7 && get<2>(robot_pay_buy[i]->work_queue.front())->full == 0){  //如果预售的和sell卖的
        //     printf("forward %d %d\n", robot_pay_buy[i]->number,-2);
        //     continue;
        // }
        int k = 8700;
        if(map_num == 1) k =8900;
        if(map_num == 3) k =8800;

        if(get<0>(robot_pay_buy[i]->work_queue.front()) == 0)printf("sell %d\n", robot_pay_buy[i]->number);
        if(get<0>(robot_pay_buy[i]->work_queue.front()) == 1 && frameID < k){
            if(map_num == 1 && frameID>8800 && get<1>(robot_pay_buy[i]->work_queue.front())<=3)continue;
            printf("buy %d\n", robot_pay_buy[i]->number);
        }

        // if(get<0>(robot_pay_buy[i]->work_queue.front()) == 0 && robot_pay_buy[i]->work_queue.size() == 1){//可以继续下一级工作
        //     if(get<2>(robot_pay_buy[i]->work_queue.front())->number == 7 && get<2>(robot_pay_buy[i]->work_queue.front())->sell_food.size() == 1){
        //         if(get<2>(robot_pay_buy[i]->work_queue.front())->sell_food.size() == 1)pla_sell_task_notake_7_89(get<2>(robot_pay_buy[i]->work_queue.front()),robot_pay_buy[i]);
        //     }else{
        //         if(get<2>(robot_pay_buy[i]->work_queue.front())->sell_food.size() == 1)pla_sell_task_notake_456_79(get<2>(robot_pay_buy[i]->work_queue.front()),robot_pay_buy[i]);
        //     }
        // }

        auto& working_numbers = get<2>(robot_pay_buy[i]->work_queue.front())->working_numbers;

        int tttttt=working_numbers.size();
        for (auto it = working_numbers.begin(); it != working_numbers.end();) {
            if (it->first == robot_pay_buy[i] &&
                it->second.first == get<0>(robot_pay_buy[i]->work_queue.front()) &&
                it->second.second == get<1>(robot_pay_buy[i]->work_queue.front())) {

                working_numbers.erase(it);
                break;
            } else {
                ++it;
            }
        }
        if(tttttt == working_numbers.size() && (get<2>(robot_pay_buy[i]->work_queue.front())->number<=7 && get<2>(robot_pay_buy[i]->work_queue.front())->number>=4)){
            cerr<<"delete working_numbers error\n";
            while(1);
        }

        int yyyyyy = robot_pay_buy[i]->work_queue.size();
        robot_pay_buy[i]->work_queue.pop();
        if(yyyyyy == robot_pay_buy[i]->work_queue.size()){
            cerr<<"delete work_queue err\n";
            while(1);
        }
        printf("forward %d %d\n",robot_pay_buy[i]->number,-2);//防止围绕着点打转
        cerr<<"6                over\n";
    }
}

void TManager::FillSellAndBuyMap(vector<TPlatform*> &last_level, vector<TPlatform*> &next_level)
{
    //填充距离map
    for(auto cost : next_level)
    {
        //遍历平台算最近
        for(auto product : last_level)
        {
            if(cost->free_numbers.find(product->number) == cost->free_numbers.end() && cost->number<=7)
                continue;

            cost->buy_map[product->number].emplace_back(make_pair(cost,product)); //先放进去，不按距离排序
            product->sell_map[cost->number].emplace_back(make_pair(product,cost));
        }
    }
}

void TManager::EraseWorkingNumber(multimap<TRobot *, std::pair<bool, int>>& wn,std::tuple<bool, int, TPlatform *>& wq_task,TRobot* ro)
{
    int tttttt=wn.size();
    for (auto it = wn.begin(); it != wn.end();) {
        if (it->first == ro &&
            it->second.first == get<0>(wq_task) &&
            it->second.second == get<1>(wq_task)) {

            wn.erase(it);
            break;
        } else {
            ++it;
        }
    }
    if(tttttt == wn.size() && (get<2>(wq_task)->number<=7 && get<2>(wq_task)->number>=4)){
        cerr<<"erase_working_number error\n";
        while(1);
    }
}

void TManager::MonitorRobot(TRobot* ro)
{
    if(ro->work_queue.size() == 0){
        return ;
    }

    if(get<0>(ro->work_queue.front()) == 0 && ro->foods.number == 0){  //是否出现没买到东西就跑了
        while(ro->work_queue.size()!=0 && get<0>(ro->work_queue.front()) == 0){
            auto& working_numbers = get<2>(ro->work_queue.front())->working_numbers;
            for (auto it = working_numbers.begin(); it != working_numbers.end();) { //擦除这个任务
                if (it->first == ro &&
                    it->second.first == get<0>(ro->work_queue.front()) &&
                    it->second.second == get<1>(ro->work_queue.front())) {
                    working_numbers.erase(it);
                    break;
                } else {
                    ++it;
                }
            }
            ro->work_queue.pop();
        }
    }
}

void TManager::WaitSellMonitorPlat7(TPlatform* plat_7)
{
    if(plat_7->pre_sel_food.size() == 0){//预售的物品是小于180帧，且没被预定的物品
        return;
    }

    map<double,TRobot*> sort_buff;
    for(auto ro: robots){
        if(ro->work_queue.size() == 0){//如果机器人为空
            sort_buff.emplace(make_pair(Distance(plat_7->x,plat_7->y,ro->x,ro->y),ro));
        }
        // else{//取消买123的任务
        //     auto &i = ro->work_queue.front();
        //     if(get<2>(i)->number<=3 && get<0>(i) == 1)
        //     {
        //         sort_buff.emplace(make_pair(distance(get<2>(i)->x,get<2>(i)->y,ro->x,ro->y),ro));
        //     }
        // }
    }

    PlaSellTaskNoTake_7_89(plat_7,sort_buff.begin()->second);
}

void TManager::SellMonitorPlat7(TPlatform* plat_7)
{
    //1安排给最近结束的机器人(最后一个任务可以是自己)    //2分钟前,不着急搬运
    if(plat_7->sell_food.size() == 0){
        return ;
    }

    map<double,TRobot*> sort_buff;
    if(plat_7->sell_food.size() == 1){   //full状态的7没有买
        for(auto ro: robots){
            if(ro->work_queue.size() == 0){//如果机器人为空
                sort_buff.emplace(make_pair(Distance(plat_7->x,plat_7->y,ro->x,ro->y)*0.5,ro));
            }else{
                int k = 1; //顺路的优先级最大
                auto &i = ro->work_queue.back();
                if(get<2>(i) == plat_7 && get<0>(i) == 0)k=0;
                sort_buff.emplace(make_pair(Distance(get<2>(i)->x,get<2>(i)->y,plat_7->x,plat_7->y)*k,ro));
                if(k==0)break;
            }
        }
    }else{ //第二马上好了
        //检查一下第二个被拿没
        int cnt = 0;
        for(auto it:plat_7->working_numbers){
            if(it.second.second == 7){
                cnt ++;
            }
        }
        if(cnt>2){
            while(1){
                cerr<<"cnt error\n";
            }
        }
        if(cnt == 2)return ;

        for(auto ro: robots){
            if(ro->work_queue.size() == 0){//如果机器人为空
                sort_buff.emplace(make_pair(Distance(plat_7->x,plat_7->y,ro->x,ro->y)*0.5,ro));
            }else{
                int k = 1; //顺路的优先级最大
                auto &i = ro->work_queue.back();
                if(get<2>(i) == plat_7 && get<0>(i) == 0)k=0;
                sort_buff.emplace(make_pair(Distance(get<2>(i)->x,get<2>(i)->y,plat_7->x,plat_7->y)*k,ro));
                if(k==0)break;
            }
        }
    }

    if(sort_buff.begin()->first > 20)return ;

    PlaSellTaskNoTake_7_89(plat_7,sort_buff.begin()->second);
}

int TManager::FullFirst(TRobot* ro)
{
    map<double,TPlatform*> sort_buff;
    for(auto it:plat_full_456){  //按照距离排序
        if(it->sell_food.size() == 0)continue;
        sort_buff.emplace(make_pair(Distance(ro->x,ro->y,it->x,it->y),it));
    }
    if(sort_buff.size() == 0)return 0;

    //检查有没有其他正在去的机器人,交给正在去的机器人
    if(map_num!=4){
        auto sb = sort_buff.begin();
        for(auto wn: sb->second->working_numbers){
            if(wn.first->work_queue.size() == 1){
                PlaSellTaskNoTake_456_79(sb->second,wn.first);
                return 0;
            }
        }
    }

    auto it_sort = sort_buff.begin();
    while(ro->work_queue.size() == 0 && it_sort!=sort_buff.end()){
        if(it_sort->second->number <= 6 && it_sort->second->sell_food.size() == 1){
            PlaSellTaskNoTake_456_79(it_sort->second,ro);
        }
        it_sort++;
    }
}

int TManager::DistanceFirst(TRobot* ro)
{//处理空闲机器人，但没买到的
    //cerr<<rob_empty_sz<<'\n';
    // TPlatform * pla_t = nullptr;
    // for(auto& it: plat_7){//先看7生产好没
    //     if(it->sell_food.size() == 1){//full，最优先！！！，尽量防止阻塞，造成车车到处走，浪费时间
    //         pla_t = it;
    //         break;
    //     }
    // }

    // if(pla_t != nullptr){
    //     pla_sell_task_notake_7_89(pla_t,ro);
    // } //这个功能挪到了sellmonitor_7

    if(ro->work_queue.size() != 0){//分配了7就返回
        return 1;
    }

    map<double,TPlatform*> sort_buff;
    for(auto it:plat_123){  //按照距离排序
        sort_buff.emplace(make_pair(Distance(ro->x,ro->y,it->x,it->y),it));
    }

    auto it_sort = sort_buff.begin();
    while(ro->work_queue.size() == 0 && it_sort!=sort_buff.end()){
        if(it_sort->second->number <=3){
            cerr<<"<3          \n";
            PlaSellTaskNoTake_123_456(it_sort->second,ro);
        }
        it_sort++;
    }

    if(ro->work_queue.size() == 0){
        cerr<<"no distrt work\n";//立马分配345full运输工作
        //while(1);
    }
    //cerr<<rob_empty_sz<<'\n';

    return 0;
}

int TManager::KSort(TPlatform* pla,TRobot* rob, double k)
{
    double dis = fabs(pla->x-rob->x)+fabs(rob->y-pla->y);
    //scerr<<dis<<'\n';
    return dis*1000*k;
}

void TManager::PlaSellTaskNoTake_123_456(TPlatform* pla,TRobot* rob)
{
    TPlatform* plat456 = nullptr;

    map<double,TPlatform*> sort_buff;
    for(auto it: pla->sell_map[4]){
        double K = 1;
        cerr<<"4 :"<<pla->sell_map[4].size()<<'\n';
        if(it.second->free_numbers.find(pla->number) != it.second->free_numbers.end()){
            if(it.second->is_full && map_num==1)K=100;
            if(it.second->is_full && map_num==3)K=0.1;
            if(map_num==4)K=0.1;
            sort_buff.emplace(make_pair(Distance(pla->x,pla->y,it.second->x,it.second->y)*((double)it.second->free_numbers.size())*K,it.second));
            break;
        }
    }
    for(auto it: pla->sell_map[5]){
        cerr<<"5 :"<<pla->sell_map[5].size()<<'\n';
        double K = 1;
        //if(map_num == 1)K=0.1;
        if(it.second->free_numbers.find(pla->number) != it.second->free_numbers.end()){
            if(it.second->is_full == 1 && map_num==1)K=100;
            if(it.second->is_full == 1 && map_num==3)K=0.1;
            sort_buff.emplace(make_pair(Distance(pla->x,pla->y,it.second->x,it.second->y)*((double)it.second->free_numbers.size())*K,it.second));
            break;
        }
    }
    for(auto it: pla->sell_map[6]){
        double K = 1;
        cerr<<"6 :"<<pla->sell_map[6].size()<<'\n';
        if(it.second->free_numbers.find(pla->number) != it.second->free_numbers.end()){
            if(it.second->is_full == 1 && map_num==1)K=100;
            if(it.second->is_full == 1 && map_num==3)K=0.1;
            sort_buff.emplace(make_pair(Distance(pla->x,pla->y,it.second->x,it.second->y)*((double)it.second->free_numbers.size())*K,it.second));
            break;
        }
    }

    // for(auto it: sort_buff){
    //     cerr<<it.first<<" "<<it.second<<'\n';
    // }
    // while(1);

    if(sort_buff.size() == 0){
        cerr<<" &&&&       \n";
        return ;
    }

    plat456 = sort_buff.begin()->second;

    cerr<<"######   \n";
    rob->work_queue.emplace(make_tuple(1,pla->number,pla));
    rob->work_queue.emplace(make_tuple(0,pla->number,plat456));

    plat456->working_numbers.emplace(rob,make_pair(0,pla->number));
    plat456->free_numbers.erase(pla->number);

    busy_robot_target_platform[rob] = 0;
}

void TManager::PlaSellTaskNoTake_456_79(TPlatform* pla,TRobot* rob)
{  //不接受预定

    TPlatform* plat9 = nullptr;

    map<double,TPlatform*> sort_buff;
    for(auto it:pla->sell_map[7]){
        if(it.second->free_numbers.find(pla->number) != it.second->free_numbers.end()){
            sort_buff.emplace(Distance(pla->x,pla->y,it.second->x,it.second->y)*it.second->free_numbers.size()*it.second->free_numbers.size(),it.second);
        }
    }

    if(pla->sell_map[9].size() != 0 && (map_num!=1 || (map_num==1&&frameID>8700)))plat9 = pla->sell_map[9].begin()->second;

    if(sort_buff.size()==0 && plat9 == nullptr)return ;

    auto plat7 = sort_buff.begin()->second;
    if(sort_buff.size() > 0){
        rob->work_queue.emplace(make_tuple(1,pla->number,pla));
        rob->work_queue.emplace(make_tuple(0,pla->number,plat7));
        pla->working_numbers.emplace(rob,make_pair(1,pla->number));
        plat7->working_numbers.emplace(rob,make_pair(0,pla->number));
        pla->sell_food.erase(pla->number);
        plat7->free_numbers.erase(pla->number);
    }else{
        if(plat9->working_numbers.size() == 2)return ;
        rob->work_queue.emplace(make_tuple(1,pla->number,pla));
        rob->work_queue.emplace(make_tuple(0,pla->number,plat9));
        pla->working_numbers.emplace(rob,make_pair(1,pla->number));

        pla->sell_food.erase(pla->number);
        plat9->working_numbers.emplace(rob,make_pair(0,pla->number));
    }
    busy_robot_target_platform[rob] = 0;
}

void TManager::PlaSellTaskNoTake_7_89(TPlatform* pla,TRobot* rob)
{
    //不接受预定
    cerr<<"3######################"<<'\n';

    TPlatform* plat8 = nullptr;
    TPlatform* plat9 = nullptr;
    TPlatform* c_plat = nullptr;

    if(pla->sell_map[8].size() != 0)plat8 = pla->sell_map[8].begin()->second;
    if(pla->sell_map[9].size() != 0)plat9 = pla->sell_map[9].begin()->second;

    int dis8 = 1000;
    int dis9 = 1000;
    if(plat8!=nullptr){
        dis8 = Distance(pla->x,pla->y,plat8->x,plat8->y);
    }
    if(plat9!=nullptr){
        dis9 = Distance(pla->x,pla->y,plat9->x,plat9->y);
    }

    if(dis8 <= dis9){
        c_plat = plat8;
    }else{
        c_plat = plat9;
    }


    rob->work_queue.emplace(make_tuple(1,pla->number,pla));
    rob->work_queue.emplace(make_tuple(0,pla->number,c_plat));

    pla->working_numbers.emplace(rob,make_pair(1,pla->number));

    if(pla->sell_food.size()==1)pla->sell_food.erase(pla->number);
    else if(pla->pre_sel_food.size()==1)pla->pre_sel_food.erase(pla->number);

    busy_robot_target_platform[rob] = 0;  //后面没用到0
//        while(1);
    cerr<<"3######################"<<'\n';
}

void TManager::Scanner(vector<vector<pair<double,double>>> &scan_line,vector<pair<double,double>> &memo_v_r)
{
    for(auto it : robots){
        double rx = it->x;
        double ry = it->y;
        double rvx = it->line_v[0];
        double rvy = it->line_v[1];
        double radv = it->rad_v;
        double rv = sqrt(pow(rvx,2) + pow(rvy,2));//合成速度
        memo_v_r.emplace_back(make_pair(rv,radv));
        if(map_num == 3){
            if(rvx < 2) rvx = 2*cos(it->direction);
            if(rvy < 2) rvy = 2*sin(it->direction);
        }

        double dx = rx + rvx*0.5; //+0.3*cos(it->direction)
        double dy = ry + rvy*0.5;//+0.3*sin(it->direction)         //这里不一定是加，有可能是减

        if(it->work_queue.empty() && get<0>(it->work_queue.front())==0){
            dx = rx + rvx*1;
            dy = ry + rvy*1;
        }

        if(dx >= rx){//对不同象限面积与小球朝向处理
            rx -= 0.3;
        }else if(dx < rx){
            rx += 0.3;
        }else if(dy >= ry){
            ry -= 0.3;
        }else{
            ry +=0.3;
        }

        scan_line[it->number][0]  = make_pair(min(dx,rx),max(dx,rx));

        scan_line[it->number][1] = make_pair(min(dy,ry),max(dy,ry));
    }
}

void TManager::Avoid(int &v,double &rotate,pair<TRobot*,int> it,vector<vector<pair<double,double>>> &scan_line,vector<pair<double,double>> &memo_v_r)
{
    for(auto rb: robots){
        if(rb->number != it.first->number){
            int x1 = scan_line[it.first->number][0].first;
            int x2 = scan_line[it.first->number][0].second;
            int y1 = scan_line[it.first->number][1].first;
            int y2 = scan_line[it.first->number][1].second;
            double vr = memo_v_r[it.first->number].first;
            double vtr = memo_v_r[it.first->number].second;//角速度
            //cerr<<x1<<x2<<y1<<y2<<vr<<endl;
            int x11 = scan_line[rb->number][0].first;
            int x22 = scan_line[rb->number][0].second;
            int y11 = scan_line[rb->number][1].first;
            int y22 = scan_line[rb->number][1].second;
            double vit = memo_v_r[rb->number].first; //线速度合成速度
            double vir = memo_v_r[rb->number].second;//角速度
            // cerr<<"aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"<<endl;
            if((x11 >= x1 && x11 < x2) || (x22 >= x1 && x22 <= x2)){//第I机器人x轴在内
                //cerr<<"bbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbb"<<endl;
                if((y11 >= y1 && y11 <= y2) ||(y22 <= y2 && y22 >= y1)){//在Y方向内
                    //cerr<<"ccccccccccccccccccc"<<endl;
                    if(rb->direction>0 && it.first->direction<0)rotate =  - M_PI;
                    else if(rb->direction<0 && it.first->direction>0)rotate =  - M_PI;

                    if(fabs(rb->direction-it.first->direction)>M_PI*0.75 && rb->direction*it.first->direction>=0) {rotate = M_PI;v=-1;}

                    if((rb->direction < M_PI / 6 && rb->direction > -M_PI/6)&& (it.first->direction > -M_PI /6 && it.first->direction < M_PI / 6)){
                        rotate = - M_PI;
                    }
                }
            }
        }
    }
}